{
  "nbformat": 4,
  "cells": [
    {
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "cell_type": "code",
      "source": [
        "%matplotlib inline"
      ],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "\n********************************************************************************\nNavigation on Flat Earth - Example\n********************************************************************************\nGoals of this script:\n\n- apply the UKF on parallelizable manifolds for estimating the 3D attitude,\n  velocity and position of a moving vehicle.\n\n*We assume the reader is already familiar with the approach described in the\ntutorial.*\n\nThis script proposes an UKF  on parallelizable manifolds to estimate the 3D\nattitude, the velocity, and the position of a rigid body in space from inertial\nsensors and relative observations of points having known locations by following\nthe setting of :cite:`barrauInvariant2017` and :cite:`VasconcelosA2010`. The\nvehicle is owed with a three axis Inertial Measurement Unit (IMU) consisting in\naccelerometers and gyroscopes. Observations of the relative position of known\nfeatures (using for instance a depth camera) are addressed.\n\n"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Import\n==============================================================================\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "cell_type": "code",
      "source": [
        "from scipy.linalg import block_diag\nimport ukfm\nimport numpy as np\nimport matplotlib\nukfm.utils.set_matplotlib_config()"
      ],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Model and Simulation\n==============================================================================\nThis script uses the :meth:`~ukfm.INERTIAL_NAVIGATION` model that requires\nthe sequence time and the IMU frequency.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "cell_type": "code",
      "source": [
        "# sequence time (s)\nT = 30\n# IMU frequency (Hz)\nimu_freq = 100\n# create the model\nmodel = ukfm.INERTIAL_NAVIGATION(T, imu_freq)"
      ],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "The true trajectory is computed along with noisy inputs after we define the\nnoise standard deviation affecting the (accurate) IMU.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "cell_type": "code",
      "source": [
        "# IMU noise standard deviation (noise is isotropic)\nimu_std = np.array([0.01,   # gyro (rad/s)\n                    0.01])  # accelerometer (m/s^2)\n# simulate true states and noisy inputs\nstates, omegas = model.simu_f(imu_std)"
      ],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "The state and the input contain the following variables:\n\n.. highlight:: python\n.. code-block:: python\n\n  states[n].Rot  # 3d orientation (matrix)\n  states[n].v    # 3d velocity\n  states[n].p    # 3d position\n  omegas[n].gyro # robot angular velocities\n  omegas[n].acc  # robot specific forces\n\n<div class=\"alert alert-info\"><h4>Note</h4><p>The IMU is assumed unbiased. IMU biases are addressed on the IMU-GNSS\n  sensor-fusion problem.</p></div>\n\n"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "We compute noisy measurements at low frequency based on the true states.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "cell_type": "code",
      "source": [
        "# observation frequency (Hz)\nobs_freq = 1\n# observation noise standard deviation (m)\nobs_std = 1\n# simulate landmark measurements\nys, one_hot_ys = model.simu_h(states, obs_freq, obs_std)"
      ],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "A measurement ``ys[k]`` contains stacked observations of all visible\nlandmarks. In this example, we have defined three landmarks that are always\nvisible.\n\n"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Filter Design and Initialization\n------------------------------------------------------------------------------\nWe now design the UKF on parallelizable manifolds. This script embeds the\nstate in $SO(3) \\times \\mathbb{R}^6$, such that:\n\n* the retraction $\\varphi(.,.)$ is the $SO(3)$ exponential for\n  orientation, and the vector addition for the vehicle velocity and position.\n\n* the inverse retraction $\\varphi^{-1}_.(.)$ is the $SO(3)$\n  logarithm for orientation and the vector subtraction for the vehicle\n  velocity and position.\n\nRemaining parameter setting is standard. The initial errors are set around 10\ndegrees for attitude and 1 meter for position in term of standard deviation.\nThese initial conditions are challenging.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "cell_type": "code",
      "source": [
        "# propagation noise covariance matrix\nQ = block_diag(imu_std[0]**2*np.eye(3), imu_std[1]**2*np.eye(3))\n# measurement noise covariance matrix\nR = obs_std**2 * np.eye(3*model.N_ldk)\n# initial uncertainty matrix such that the state is not perfectly initialized\nP0 = block_diag((10*np.pi/180)**2*np.eye(3), np.zeros((3, 3)), np.eye(3))\n# sigma point parameters\nalpha = np.array([1e-3, 1e-3, 1e-3])\n# start by initializing the filter with an unaccurate state\nstate0 = model.STATE(\n    Rot=ukfm.SO3.exp(10*np.pi/180*np.ones(3)/3).dot(states[0].Rot),\n    v=states[0].v,\n    p=states[0].p + np.array([1, 0.5, 0.7]))\n# create the UKF\nukf = ukfm.UKF(state0=state0, P0=P0, f=model.f, h=model.h, Q=Q, R=R,\n               phi=model.phi, phi_inv=model.phi_inv, alpha=alpha)\n# set variables for recording estimates along the full trajectory\nukf_states = [state0]\nukf_Ps = np.zeros((model.N, 9, 9))\nukf_Ps[0] = P0"
      ],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Filtering\n==============================================================================\nThe UKF proceeds as a standard Kalman filter with a for loop.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "cell_type": "code",
      "source": [
        "# measurement iteration number\nk = 1\nfor n in range(1, model.N):\n    # propagation\n    ukf.propagation(omegas[n-1], model.dt)\n    # update only if a measurement is received\n    if one_hot_ys[n] == 1:\n        ukf.update(ys[k])\n        k = k + 1\n    # save estimates\n    ukf_states.append(ukf.state)\n    ukf_Ps[n] = ukf.P"
      ],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Results\n------------------------------------------------------------------------------\nWe plot the trajectory, the position of the landmarks and the estimated\ntrajectory in the same plot, the attitude error, the position error, and their\nconfidence interval.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "outputs": [],
      "cell_type": "code",
      "source": [
        "model.plot_results(ukf_states, ukf_Ps, states)"
      ],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "It seems that the proposed UKF meets difficulties and takes some time to\nconverge due to the challenging initial conditions. A major problem of the UKF\n(in this problem and with this choice of retraction) is to be particularly\noveroptimism regarding attitude error, which is clearly outside the confidence\nintervals.\n\n"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Conclusion\n==============================================================================\nThis script readily implements an UKF for estimating the 3D pose and velocity\nof a platform. Results are not particularly satisfying, since the filter\ndifficultly converges to the true state even at the end of the trajectory. But\nis it not possible to improve the filter accuracy and consistency performances\nby inflating sensor noise parameters of the filter, or better, by defining a\nnew retraction more adapted to the considered problem ?\n\nYou can now:\n\n* benchmark the UKF and compare it to the extended Kalman filter and the\n  invariant extended Kalman filter of :cite:`barrauInvariant2017`.\n\n* modify the model with a non-linear range and bearing measurement.\n\n* add and estimate sensor biases on the gyro and accelerometer measurements.\n\n"
      ]
    }
  ],
  "metadata": {
    "language_info": {
      "nbconvert_exporter": "python",
      "name": "python",
      "version": "3.5.2",
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "pygments_lexer": "ipython3"
    },
    "kernelspec": {
      "name": "python3",
      "language": "python",
      "display_name": "Python 3"
    }
  },
  "nbformat_minor": 0
}